#include "costmap_base.h"

#include "costmap_base.h"

namespace costmap_2d{

Costmap::Costmap(){

}

Costmap::~Costmap(){

}

// 添加地图以及相关数据
void Costmap::setMap(nav_msgs::OccupancyGrid::Ptr map_ptr){
    mapOriginPtr = map_ptr;
    map_metadata = map_ptr->info;
    costMap.resize(map_ptr->data.size(), costmap_2d::FREE_SPACE);
    for(int i=0; i<map_ptr->data.size(); ++i){
        int8_t value = map_ptr->data[i];
        if(value >= 70){        //障碍物
            costMap[i] = costmap_2d::LETHAL_OBSTACLE;
        }
    }
}


void Costmap::indexToMap(int index, int& mx, int& my){
    my = index / this->map_metadata.width;
    mx = index - (my * this->map_metadata.width);
}

int Costmap::mapToIndex(int mx, int my){
    return my * this->map_metadata.width + mx;
}

void Costmap::mapToWorld(int mx, int my, double& wx, double& wy){
    wx = this->map_metadata.origin.position.x + (mx + 0.5) * this->map_metadata.resolution;
    wy = this->map_metadata.origin.position.y + (my + 0.5) * this->map_metadata.resolution;
}

void Costmap::worldToMap(double wx, double wy, int& mx, int& my){
    mx = int((wx - this->map_metadata.origin.position.x) / this->map_metadata.resolution);
    my = int((wy - this->map_metadata.origin.position.y) / this->map_metadata.resolution);
    // cout << "[worldToMap] mx: " << mx << ", my: " << my << endl;
}

void Costmap::opencvMapToWorld(int mx, int my, double& wx, double& wy){
    my = this->map_metadata.height - 1 - my; 
    wx = this->map_metadata.origin.position.x + (mx + 0.5) * this->map_metadata.resolution;
    wy = this->map_metadata.origin.position.y + (my + 0.5) * this->map_metadata.resolution;
}

void Costmap::worldToOpencvMap(double wx, double wy, int& mx, int& my){
    mx = int((wx - this->map_metadata.origin.position.x) / this->map_metadata.resolution);
    my = int((wy - this->map_metadata.origin.position.y) / this->map_metadata.resolution);
    // cout << "[worldToMap] mx: " << mx << ", my: " << my << endl;
    my = this->map_metadata.height - 1 - my; 
}

bool Costmap::isValid(int mx, int my){
    if(mx < this->map_metadata.width && my < this->map_metadata.height)
        return true;
    return false;
}

bool Costmap::isValid(int index){
    if(index < this->costMap.size())
        return true;
    return false;
}

// 是否为障碍物
bool Costmap::valueIsValid(int mx, int my){
    unsigned char cost = getCost(mx, my);
    // cout << "mx: " << mx << ", my: " << my << ", cost: " << int(cost) << endl;
    if(cost == costmap_2d::UNKNOWN){
        return false;
    }
    else{
        if(cost > costmap_2d::FREE_SPACE) return false;
    }
    // if(cost > costmap_2d::FREE_SPACE && cost != costmap_2d::UNKNOWN) return false;
    return true;
}

unsigned char Costmap::getCost(int mx, int my){
    if(isValid(mx, my)){
        return this->costMap[mapToIndex(mx, my)];
    }
    return costmap_2d::UNKNOWN;
}

unsigned char Costmap::getCost(int index){
    if(index >= this->costMap.size()){
        cout << "[getCost] index: " << index << ", size: " << this->costMap.size() << endl;
    }
    if(isValid(index)){
        return this->costMap[index];
    }
    return costmap_2d::UNKNOWN;
}


}; //costmap_2d